# -*- coding: utf-8 -*-
from abc import ABCMeta
import logging
import numpy as np


# class Contact(metaclass=ABCMeta):  # for python3
class Contact:
    """ Abstract class for contact models. """
    __metaclass__ = ABCMeta


class Contact3D(Contact):
    """ 3D contact points.

    Attributes
    ----------
    graspable : :obj:`GraspableObject3D`
        object to use to get contact information
    contact_point : 3x1 :obj:`numpy.ndarray`
        point of contact on the object
    in_direction : 3x1 :obj:`numpy.ndarray`
        direction along which contact was made
    normal : normalized 3x1 :obj:`numpy.ndarray`
        surface normal at the contact point
    """
    def __init__(self, graspable, index=None, contact_point=None, normal=None, in_direction=None):
        self.graspable_ = graspable
        assert contact_point is not None
        self.point_ = contact_point
        if normal is None:
            assert index is not None 
        if index is not None:
            self.tri_index_ = index
            point_index = graspable.mesh.faces[self.tri_index_]
            total = graspable.mesh.vertex_faces[point_index].flatten()
            total = np.unique(total)
            total = np.setdiff1d(total, -1)
            normal = graspable.mesh.face_normals[total]
            face_area = graspable.mesh.area_faces[total]
            mean_vec = [x * y for x, y in zip(normal, face_area)]
            normal_ = np.array(mean_vec).sum(axis=0)/face_area.sum()
            normal_ /= np.linalg.norm(normal_)
            # print('point position is : ', self.point_)
            self.normal_ = normal_  # outward facing normal
            self.curvature_ = graspable.mean_curvature[point_index].mean()

        else:
            print('*************** Reach Here ***************')
            self.normal_ = normal
            nearest_point_index = graspable.mesh_tree.query(self.point_, k=1)[1]
            self.curvature_ = graspable.mean_curvature[nearest_point_index]

        # cached attributes
        self.in_direction_ = in_direction  # inward facing grasp axis
        self.friction_cone_ = None
        self.in_noraml = None
        self.t1 = None
        self.t2 = None
        self.surface_info_ = None

#        self._compute_normal()

    @property
    def graspable(self):
        return self.graspable_

    @property
    def point(self):
        return self.point_

    @property
    def curvature(self):
        return self.curvature_

    @property
    def curvature_score(self):
        if self.curvature_ > 0:
            score = 1 - self.curvature_/self.graspable_.threshold
            return score if score > 0 else 0
        else:
            score = 1 - abs(self.curvature_)/(self.graspable_.threshold*1)  # 3/4
            return score if score > 0 else 0

    @property
    def normal(self):
        return self.normal_

    @normal.setter
    def normal(self, normal):
        self.normal_ = normal

    @property
    def in_direction(self):
        return self.in_direction_

    def tangents(self, direction=None, align_axes=True, max_samples=1000):
        """Returns the direction vector and tangent vectors at a contact point.
        The direction vector defaults to the *inward-facing* normal vector at
        this contact.
        The direction and tangent vectors for a right handed coordinate frame.

        Parameters
        ----------
        direction : 3x1 :obj:`numpy.ndarray`
            direction to find orthogonal plane for
        align_axes : bool
            whether or not to align the tangent plane to the object reference frame
        max_samples : int
            number of samples to use in discrete optimization for alignment of reference frame

        Returns
        -------
        direction : normalized 3x1 :obj:`numpy.ndarray`
            direction to find orthogonal plane for
        t1 : normalized 3x1 :obj:`numpy.ndarray`
            first tangent vector, x axis
        t2 : normalized 3x1 :obj:`numpy.ndarray`
            second tangent vector, y axis
        """
        # illegal contact, cannot return tangents
        if self.normal_ is None:
            return None, None, None

        # default to inward pointing normal
        if direction is None:
            direction = -self.normal_

        # force direction to face inward
        if np.dot(self.normal_, direction) > 0:
            direction = -direction

        # transform to 
        direction = direction.reshape((3, 1))  # make 2D for SVD

        # get orthogonal plane
        U, _, _ = np.linalg.svd(direction)

        # U[:, 1:] spans the tanget plane at the contact
        x, y = U[:, 1], U[:, 2]

        # make sure t1 and t2 obey right hand rule
        z_hat = np.cross(x, y)
        if z_hat.dot(direction) < 0:
            y = -y
        v = x
        w = y

        # redefine tangent x axis to automatically align with the object x axis
        if align_axes:
            max_ip = 0
            max_theta = 0
            target = np.array([1, 0, 0])
            theta = 0
            d_theta = 2 * np.pi / float(max_samples)
            for i in range(max_samples):
                v = np.cos(theta) * x + np.sin(theta) * y
                if v.dot(target) > max_ip:
                    max_ip = v.dot(target)
                    max_theta = theta
                theta = theta + d_theta

            v = np.cos(max_theta) * x + np.sin(max_theta) * y
            w = np.cross(direction.ravel(), v)
        self.in_normal, self.t1, self.t2 = np.squeeze(direction), v, w
        return np.squeeze(direction), v, w

    def normal_force_magnitude(self):
        """ Returns the component of the force that the contact would apply along the normal direction.

        Returns
        -------
        float
            magnitude of force along object surface normal
        """
        normal_force_mag = 1.0
        if self.in_direction_ is not None and self.normal_ is not None:
            in_normal = -self.normal_
            in_direction_norm = self.in_direction_ / np.linalg.norm(self.in_direction_)
            normal_force_mag = np.dot(in_direction_norm, in_normal)
        return max(normal_force_mag, 0.0)

    def friction_cone(self, num_cone_faces=8, friction_coef=0.5):
        """ Computes the friction cone and normal for a contact point.

        Parameters
        ----------
        num_cone_faces : int
            number of cone faces to use in discretization
        friction_coef : float 
            coefficient of friction at contact point
        
        Returns
        -------
        success : bool
            False when cone can't be computed
        cone_support : :obj:`numpy.ndarray`
            array where each column is a vector on the boundary of the cone
        normal : normalized 3x1 :obj:`numpy.ndarray`
            outward facing surface normal
        """
        if self.friction_cone_ is not None and self.normal_ is not None:
            return True, self.friction_cone_, self.normal_

        # get normal and tangents
        if self.t1 is None:
           in_normal, t1, t2 = self.tangents()
        if self.in_normal is None:
            return False, self.friction_cone_, self.normal_

        # check whether contact would slip, which is whether or not the tangent force is always
        # greater than the frictional force
        if self.in_direction_ is not None:
            in_direction_norm = self.in_direction_ / np.linalg.norm(self.in_direction_)
            normal_force_mag = self.normal_force_magnitude()
            tan_force_x = np.dot(in_direction_norm, t1)
            tan_force_y = np.dot(in_direction_norm, t2)
            tan_force_mag = np.sqrt(tan_force_x ** 2 + tan_force_y ** 2)
            friction_force_mag = friction_coef * normal_force_mag

            if friction_force_mag < tan_force_mag:
                logging.debug('Contact would slip')
                return False, self.friction_cone_, self.normal_

        # set up friction cone
        force = self.in_normal
        cone_support = np.zeros((3, num_cone_faces))

        # find convex combinations of tangent vectors
        for j in range(num_cone_faces):
            tan_vec = self.t1 * np.cos(2 * np.pi * (float(j) / num_cone_faces)) + self.t2 * np.sin(
                2 * np.pi * (float(j) / num_cone_faces))
            cone_support[:, j] = force + friction_coef * tan_vec

        self.friction_cone_ = cone_support
        return True, self.friction_cone_, self.normal_

    def torques(self, forces):
        """
        Get the torques that can be applied by a set of force vectors at the contact point.

        Parameters
        ----------
        forces : 3xN :obj:`numpy.ndarray`
            the forces applied at the contact

        Returns
        -------
        success : bool
            whether or not computation was successful
        torques : 3xN :obj:`numpy.ndarray`
            the torques that can be applied by given forces at the contact
        success : bool
            whether or not computation was successful
        torques : 3xN :obj:`numpy.ndarray`
            the torques that can be applied by given forces at the contact
        """

        num_forces = forces.shape[1]
        torques = np.zeros([3, num_forces])
        moment_arm = self.graspable.moment_arm(self.point)
        for i in range(num_forces):
            torques[:, i] = np.cross(moment_arm, forces[:, i])
        return True, torques